#include "hardware.h"

module DisplayP {
  provides interface Display;
  provides interface Init;
  uses interface Lcd1602;
  uses interface Init as Lcd1602_init;
  uses interface Timer<TMilli> as refresh;
  uses interface Fly4000_Dbg_Leds;
  }

implementation {

#define _LINE_SIZE 32
#define _LINE_CNT 2

typedef struct {
   char display_buf[_LINE_SIZE];
   uint8_t length;
   }t_display_line;


   t_display_line _line[_LINE_CNT];
   uint8_t _cur_line;

#define _LINE_NEXT ((++_cur_line)%_LINE_CNT)

  /**
   * Initialization
   */
  command error_t Init.init() {
	uint8_t i = 0;
      call Lcd1602_init.init();

	for (i=0;i<_LINE_CNT;i++) {
		memset(_line[i].display_buf, 0, _LINE_SIZE);
		_line[i].length = 0;
	}

	_cur_line = 0;

//    call refresh.startPeriodic( 1000 );
//    call Fly4000_Dbg_Leds.led1Toggle();
    return SUCCESS;
  }

  event void refresh.fired()  {
      char* ptr;
      call Fly4000_Dbg_Leds.led0Toggle();
      
/*
      ptr = _line[_cur_line].display_buf;
	call Lcd1602.CgramDdramDataWrite(*(ptr++));
      while (*ptr) {
          call Lcd1602.CgramDdramDataWrite(*(ptr++));
      }
      ptr = _line[(_cur_line+1)%_LINE_CNT].display_buf;
      while (*ptr) {
          call Lcd1602.CgramDdramDataWrite(*(ptr++));
      }
      */
  }
  
  /**
   * Screen clear
   */
  async command void Display.ScreenClear() {
      call Lcd1602.ScreenClear();
  }

  /**
   * New Line
   */
  async command void Display.NewLine(){
  }

  /**
   * Print Message
   */
  async command void Display.Print(char* string) {
  atomic {
	char* ptr = string;
	uint8_t i = 0;
	while(*ptr) {
	       call Lcd1602.CgramDdramDataWrite(*ptr);
		_line[_cur_line].display_buf[i++] = *ptr++;
	}

//    call Lcd1602.CgramDdramDataWrite(_line[_cur_line].display_buf[0]);
//	call Lcd1602.CgramDdramDataWrite(display_buf[0]);
//	call Lcd1602.CgramDdramDataWrite(*string);
//	_LINE_NEXT;
	}
  }

  /**
   * Set Cursor
   *
   */
  async command void Display.SetCursor(uint8_t position) {
  }
	

}
